//
// Created by ze on 2023/11/8.
//
#include "UPRE_MOVEBASE.h"
#include "UPRE_LOCATION.h"
#include "UPRE_MOTOR.h"
#include "UPRE_PATH.h"
#include "UPRE_PID.h"
#include "UPRE_Hand_Shank.h"
Wheel_Name_Measure Wheel_Totaldir;

/* 底盘参数初始化*/
/**********************************************************************************************************/
//四舵轮底盘
static WheelSys MoveBase = {
        .LeftFront = {.RotationRadius = 1.f, .PositiveDir = 135.f,.currentPos = 0, .targetVel = {0}},//1    PositiveDIr是安装位置(正右方为0，顺负逆正)
        .RightFront = {.RotationRadius = 1.f, .PositiveDir = 45.f, .currentPos = 0, .targetVel = {0}},//2
        .RightRear = {.RotationRadius = 1.f, .PositiveDir = -45.f, .currentPos = 0, .targetVel = {0}},//3
        .LeftRear = {.RotationRadius = 1.f, .PositiveDir = -135.f, .currentPos = 0, .targetVel = {0}},//4
};

//四全向轮底盘
static WheelSys OminiBase = {
        .LeftFront = {.RotationRadius = 1.f, .PositiveDir = 135.f,.currentPos = 0, .targetVel = {0}},//1
        .LeftRear = {.RotationRadius = 1.f, .PositiveDir = -135.f, .currentPos = 0, .targetVel = {0}},//2
        .RightRear = {.RotationRadius = 1.f, .PositiveDir = -45.f, .currentPos = 0, .targetVel = {0}},//3
        .RightFront = {.RotationRadius = 1.f, .PositiveDir = 45.f, .currentPos = 0, .targetVel = {0}},//4
};
//三全向轮底盘
static WheelSys OminiBase3 = {
        .LeftFront = {.RotationRadius = 1.f, .PositiveDir = 90.f,.currentPos = 0, .targetVel = {0}},//1
        .RightRear = {.RotationRadius = 1.f, .PositiveDir = -30.f,.currentPos = 0, .targetVel = {0}},//2
        .LeftRear = {.RotationRadius = 1.f, .PositiveDir  = -150.f, .currentPos = 0, .targetVel = {0}},//3
};

/**
 * @brief 轮子输出速度
 * @param
*/
static void Output_Speed(WheelSys WheelType)
{
#if MODE == 0
    wb_motor_set_velocity(wheel_name_vel[1], -WheelType.LeftFront.targetVel.size);
    wb_motor_set_velocity(wheel_name_vel[2], -WheelType.RightFront.targetVel.size);
    wb_motor_set_velocity(wheel_name_vel[3], -WheelType.RightRear.targetVel.size);
    wb_motor_set_velocity(wheel_name_vel[4], -WheelType.LeftRear.targetVel.size);
#endif // MODE == 0

#if MODE == 1

    //    wb_motor_set_velocity(wheel_name_vel[1], -WheelType.LeftFront.targetVel.size );
//    wb_motor_set_velocity(wheel_name_vel[2], -WheelType.RightFront.targetVel.size);
//    wb_motor_set_velocity(wheel_name_vel[3], -WheelType.RightRear.targetVel.size );
//    wb_motor_set_velocity(wheel_name_vel[4], -WheelType.LeftRear.targetVel.size  );
//    Motor_Speed((int32_t)(WheelType.LeftFront.targetVel.size),
//                (int32_t)(WheelType.LeftRear.targetVel.size),
//                (int32_t)(WheelType.RightRear.targetVel.size),
//                (int32_t)(WheelType.RightFront.targetVel.size));

    DjiMotor_Speed((int32_t)(WheelType.LeftFront.targetVel.size),
                   (int32_t)(WheelType.LeftRear.targetVel.size),
                   (int32_t)(WheelType.RightRear.targetVel.size),
                   (int32_t)(WheelType.RightFront.targetVel.size));
#if Printf_MotorSpeed == 1
    printf("vel1:%f\tvel2:%f\tvel3:%f\tvel4:%f\n",WheelType.LeftFront.targetVel.size,WheelType.LeftRear.targetVel.size,WheelType.RightRear.targetVel.size,WheelType.RightFront.targetVel.size);
#endif

#endif // MODE == 1



}

/**
 * @brief 判断是否有轮子超速
 * @param
*/
static void speedLimit(WheelSys WheelType)
{
    Limit_Max(Limit_Min(WheelType.LeftFront.targetVel.size,  -MAXSPEED),MAXSPEED);
    Limit_Max(Limit_Min(WheelType.LeftRear.targetVel.size,  -MAXSPEED),MAXSPEED);
    Limit_Max(Limit_Min(WheelType.RightRear.targetVel.size,  -MAXSPEED),MAXSPEED);
    Limit_Max(Limit_Min(WheelType.RightFront.targetVel.size,  -MAXSPEED),MAXSPEED);

}

/*********舵轮*****************舵轮*******************舵轮*********************舵轮************************************************************************************************/
/**
 * @brief get_total_dir   //获得转过的总角度
 * @param motor_measure* p
 * @return  获得转过的总角度
*/
static float get_total_dir(motor_measure p)
{

    if (p.dir > p.last_dir)
    {
        p.total_dir += p.dir - p.last_dir;
    }
    else
    {
        p.total_dir -= p.last_dir - p.dir;
    }



    return p.total_dir;
}
/**
 * @brief 轮子输出角度(弧度制)
 * @param
*/
static void Output_dir(void)
{
#if MODE == 0
    Wheel_Totaldir.Wheel1_Measure.dir = Radian2Angle(wb_motor_get_target_position(wheel_name_dir[1]));
    Wheel_Totaldir.Wheel2_Measure.dir = Radian2Angle(wb_motor_get_target_position(wheel_name_dir[2]));
    Wheel_Totaldir.Wheel3_Measure.dir = Radian2Angle(wb_motor_get_target_position(wheel_name_dir[3]));
    Wheel_Totaldir.Wheel4_Measure.dir = Radian2Angle(wb_motor_get_target_position(wheel_name_dir[4]));


    Wheel_Totaldir.Wheel1_Measure.total_dir = get_total_dir(Wheel_Totaldir.Wheel1_Measure);
    Wheel_Totaldir.Wheel2_Measure.total_dir = get_total_dir(Wheel_Totaldir.Wheel2_Measure);
    Wheel_Totaldir.Wheel3_Measure.total_dir = get_total_dir(Wheel_Totaldir.Wheel3_Measure);
    Wheel_Totaldir.Wheel4_Measure.total_dir = get_total_dir(Wheel_Totaldir.Wheel4_Measure);

    Wheel_Totaldir.Wheel1_Measure.last_dir = Wheel_Totaldir.Wheel1_Measure.dir;
    Wheel_Totaldir.Wheel2_Measure.last_dir = Wheel_Totaldir.Wheel2_Measure.dir;
    Wheel_Totaldir.Wheel3_Measure.last_dir = Wheel_Totaldir.Wheel3_Measure.dir;
    Wheel_Totaldir.Wheel4_Measure.last_dir = Wheel_Totaldir.Wheel4_Measure.dir;
#if PrintfTotalDir == 1
    printf("Total1:%f\n", Wheel_Totaldir.Wheel1_Measure.total_dir);
    printf("Total2:%f\n", Wheel_Totaldir.Wheel2_Measure.total_dir);
    printf("Total3:%f\n", Wheel_Totaldir.Wheel3_Measure.total_dir);
    printf("Total4:%f\n", Wheel_Totaldir.Wheel4_Measure.total_dir);
#endif


    wb_motor_set_position(wheel_name_dir[1], Angle2Radian(MoveBase.LeftFront.targetVel.dir));
    wb_motor_set_position(wheel_name_dir[2], Angle2Radian(MoveBase.RightFront.targetVel.dir));
    wb_motor_set_position(wheel_name_dir[3], Angle2Radian(MoveBase.RightRear.targetVel.dir));
    wb_motor_set_position(wheel_name_dir[4], Angle2Radian(MoveBase.LeftRear.targetVel.dir));
#endif
}


/**
  * @brief  calSumWheelVel 计算合速度
  * @note
  * @param  transX：  X方向的平移速度
  * @param  transY：  Y方向的平移速度
  * @param  linearVel：线速度
  * @retval void
  **/

static void calSumWheelVel(WheelParam* pwheelParam, float transX, float transY, float lineVel)
{
    //分别计算X、Y方向的分速度
    float sumVelX = transX + lineVel * cosf(Angle2Radian(pwheelParam->PositiveDir));
    float sumVelY = transY + lineVel * sinf(Angle2Radian(pwheelParam->PositiveDir));

    //计算合速度大小
    pwheelParam->targetVel.size = sqrtf(sumVelX * sumVelX + sumVelY * sumVelY);

    //零向量判定
    (pwheelParam->targetVel).dir = (pwheelParam->targetVel).size > FLOAT_ZERO ? Radian2Angle(atan2f(sumVelY, sumVelX)) : (pwheelParam->currentPos/* + 90.f */);//(此处+90) 与车系轮系之间建立的坐标关系有关

    //将合速度转为轮系坐标下的速度
    //pwheelParam->targetVel.dir -= 90.f;

    //限制180范围
    pwheelParam->targetVel.dir = AngleLimit180(pwheelParam->targetVel.dir);

}

/*
  * @brief  calWheelVel 计算每个轮子的速度
  * @note
  * @param  wheelSysVel：需要赋值轮系速度结构体变量
  * @param  vel：		 机器人坐标系的速度
  * @param  omega：		 角速度
  * @retval void
  */
static void calWheelVel( vector* vel, float omega)
{
    if (vel != NULL)
    {

        // 1.分解平移速度
        float translationSpeedX = vel->size * cosf(Angle2Radian(vel->dir));
        float translationSpeedY = vel->size * sinf(Angle2Radian(vel->dir));
        // 2.计算轮子的线速度(顺正逆负)
        float LFLineVel = omega * (MoveBase.LeftFront.RotationRadius);
        float RFLineVel = omega * (MoveBase.RightFront.RotationRadius);
        float RRLineVel = omega * (MoveBase.RightRear.RotationRadius);
        float LRLineVel = omega * (MoveBase.LeftRear.RotationRadius);
        MoveBase.LeftFront.currentPos = vel->dir;
        MoveBase.RightFront.currentPos = vel->dir;
        MoveBase.RightRear.currentPos = vel->dir;
        MoveBase.LeftRear.currentPos = vel->dir;
        // 3.分别计算每个轮子的合速度(每个轮子需要给平移速度和自旋线速度) 并限制转角范围为-180~180
        calSumWheelVel(&(MoveBase.LeftFront),  translationSpeedX, translationSpeedY,  LFLineVel);
        calSumWheelVel(&(MoveBase.RightFront), translationSpeedX, translationSpeedY,  RFLineVel);
        calSumWheelVel(&(MoveBase.RightRear),  translationSpeedX, translationSpeedY,  RRLineVel);
        calSumWheelVel(&(MoveBase.LeftRear),   translationSpeedX, translationSpeedY,  LRLineVel);

    }
}

/**
  * @brief  wheelTurnMindir 优化转向电机转角
  * @note
  * @param  targetVel：轮子的速度
  * @param  currentMotorPos：当前电机的位置
  * @param  wheelId：对应方向的轮子
  * @retval void
  */
void wheelTurnMindir(vector* targetVel, float currentMotorPos)
{
    if (targetVel != NULL)
    {
        // 计算与目标位置中心对称位置
        targetVel->dir = AngleLimit180(targetVel->dir);
        float oppositePos = AngleLimit180(targetVel->dir + 180.f);

        // 将目标位置转换为同一个周期
        targetVel->dir = Angle2SameCycle180(targetVel->dir, currentMotorPos);
        oppositePos = Angle2SameCycle180(oppositePos, currentMotorPos);

        // 根据轮子位置反馈，找到离轮子正方向最近的那个角，因为两条相交直线的夹角总有一个是小于等于九十度的角
        float deltadir = AngleLimit180(targetVel->dir - currentMotorPos);
        float deltaOpdir = AngleLimit180(oppositePos - currentMotorPos);
        float absDeltadir = fabsf(deltadir);
        float absDeltaOpdir = fabsf(deltaOpdir);

        // 核心逻辑，增量法
        // 若原目标位置本来就是离电机位置最近
        if (absDeltadir <= absDeltaOpdir)
        {
            targetVel->dir = currentMotorPos + deltadir;
        }
            // 若目标位置的中心对称位置离电机位置最近, 速度反向
        else
        {
            targetVel->dir = currentMotorPos + deltaOpdir;
            targetVel->size = -targetVel->size;
        }

        //printf("deltadir:%f\ndeltaOpdir:%f\ncurrentMotorPos:%f\ntargetDir:%f\n", deltadir,deltaOpdir,currentMotorPos, targetVel->dir);
    }
}
/**
  * @brief  wheelSysPosJuge 优化转向电机转角
  * @note
  * @param  wheelSysVel：轮系的速度
  * @retval void
  */
static void wheelsysPosJuge(void)
{
    MoveBase.LeftFront.currentPos = Wheel_Totaldir.Wheel1_Measure.total_dir;
    MoveBase.RightFront.currentPos = Wheel_Totaldir.Wheel2_Measure.total_dir;
    MoveBase.RightRear.currentPos = Wheel_Totaldir.Wheel3_Measure.total_dir;
    MoveBase.LeftRear.currentPos = Wheel_Totaldir.Wheel4_Measure.total_dir;
    //printf("tDir1:%f\n", Wheel_Totaldir.Wheel1_Measure.total_dir);
    //printf("tDir2:%f\n", Wheel_Totaldir.Wheel2_Measure.total_dir);
    //printf("tDir3:%f\n", Wheel_Totaldir.Wheel3_Measure.total_dir);
    //printf("tDir4:%f\n", Wheel_Totaldir.Wheel4_Measure.total_dir);

    wheelTurnMindir(&(MoveBase.LeftFront.targetVel), MoveBase.LeftFront.currentPos);
    wheelTurnMindir(&(MoveBase.RightFront.targetVel), MoveBase.RightFront.currentPos);
    wheelTurnMindir(&(MoveBase.RightRear.targetVel), MoveBase.RightRear.currentPos);
    wheelTurnMindir(&(MoveBase.LeftRear.targetVel), MoveBase.LeftRear.currentPos);
}

/******************************************************************************************************************************************************************/
/**************************全向轮**************全向轮**************全向轮****************全向轮********************************************************************************************/

static void WheelSysMotorOutput(WheelSys WheelType)
{
#if MODE == 0
    Output_dir();

#endif // MODE == 0

    speedLimit(WheelType);
    Output_Speed(WheelType);
}




/**
 * @brief 全向轮解算
 * @param targetVel
 * @param omega
*/
static void calOminiVel(vector targetVel, float omega,WheelSys type)
{
    static float TranslationX = 0;
    static float TranslationY = 0;


    TranslationX = targetVel.size * cosf(Angle2Radian(targetVel.dir));

    TranslationY = targetVel.size * sinf(Angle2Radian(targetVel.dir));
//绝对
    type.LeftFront.targetVel.size = TranslationX * cosf(Angle2Radian( type.LeftFront.PositiveDir)    +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY *  sinf(Angle2Radian(  type.LeftFront.PositiveDir)   +  Angle2Radian( Robot_Pose.yaw))   + omega ;
    type.LeftRear.targetVel.size = TranslationX * cosf(Angle2Radian(  type.LeftRear.PositiveDir)     +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY *   sinf(Angle2Radian(type.LeftRear.PositiveDir)     +  Angle2Radian( Robot_Pose.yaw))   + omega ;
    type.RightRear.targetVel.size = TranslationX * cosf(Angle2Radian( type.RightRear.PositiveDir)    +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY *  sinf(Angle2Radian(  type.RightRear.PositiveDir)   +  Angle2Radian( Robot_Pose.yaw))   + omega ;
    type.RightFront.targetVel.size = TranslationX * cosf(Angle2Radian(type.RightFront.PositiveDir)   +  Angle2Radian( Robot_Pose.yaw) ) + TranslationY * sinf(Angle2Radian(    type.RightFront.PositiveDir) +  Angle2Radian( Robot_Pose.yaw))   + omega ;
//
//相对
//    type.LeftFront.targetVel.size = TranslationX * cosf(Angle2Radian( type.LeftFront.PositiveDir)     ) + TranslationY * sinf(Angle2Radian(type.LeftFront.PositiveDir ) )   + omega * 40.f;
//    type.LeftRear.targetVel.size = TranslationX * cosf(Angle2Radian(  type.LeftRear.PositiveDir)      ) + TranslationY * sinf(Angle2Radian(type.LeftRear.PositiveDir  ) )  + omega * 40.f;
//    type.RightRear.targetVel.size = TranslationX * cosf(Angle2Radian( type.RightRear.PositiveDir)     ) + TranslationY * sinf(Angle2Radian(type.RightRear.PositiveDir ) )  + omega * 40.f;
//    type.RightFront.targetVel.size = TranslationX * cosf(Angle2Radian(type.RightFront.PositiveDir)    ) + TranslationY * sinf(Angle2Radian(type.RightFront.PositiveDir) )  + omega * 40.f;
    WheelSysMotorOutput(type);
}



/**
  * @brief  wheelSysOutput 运动解算入口函数
  * @note
  * @param  vel：		车体坐标的速度
  * @param	omega： 角速度
  * @retval void
  */
void wheelSysOutPut(vector vel, float omega)
{
    //1.世界坐标角度转成机器人坐标系角度
#if MODE == 0

    vel.dir -= getCurrentRobPose().yaw;
#endif // MODE == 0


    //printf("yaw:%f\n", getCurrentRobPose().yaw);
#if MODE == 0   //舵轮

    //2.在机器人坐标系下,计算每个轮子的速度,将四个轮子看为四个矢量
    calWheelVel(&vel, omega);

    //3.优化转向电机转角
    wheelsysPosJuge();
    //4.输出到轮系电机上
    WheelSysMotorOutput(MoveBase);

#endif
#if MODE == 1   //全向轮
    calOminiVel(vel,omega,OminiBase);

//    WheelSysMotorOutput(OminiBase);

#endif
}
//机器人底盘运动总输出
void robotMoveTask()
{

//    //平移速度
//    static vector goPathVel = { 0 };
//    static ChassisMovement  adjustVector = { 0 };
//    static vector endVel = { 0 };
//    static float omega = 0.f;
//#ifdef BlueTeeth
//
//    goPathVel = BlueTeethVel;
//#else
//    goPathVel = getPathVel();
//#endif
//#if Printf_TargetVel == 1
//    printf("goPathVel:%f   adjustVector:%f   omega:%f\n", goPathVel.size, adjustVector.transVel.size, omega);
//#endif
//
//    adjustVector = getPidOutput();
//    omega = adjustVector.omega;
//
//    endVel = vectorSynthesis(&adjustVector.transVel, &goPathVel);
//    wheelSysOutPut(endVel, omega);
//    speedLimit(OminiBase);
    //平移速度
    static vector goPathVel = { 0 };
    static ChassisMovement  adjustVector = { 0 };
    static vector endVel = { 0 };
    static float omega = 0.f;
if(Hand_Shank_Data.Flag_Start_HandShank == 1)
{
    goPathVel = Hand_Shank_Data.Vel_Hand_Shank;

}
else
{
    goPathVel = getPathVel();
}

    adjustVector = getPidOutput();
    omega = adjustVector.omega;
    endVel = vectorSynthesis(&adjustVector.transVel, &goPathVel);
    wheelSysOutPut(endVel, omega);
    speedLimit(OminiBase);
//    printf("omega:%f\r\n",omega);
#if Printf_TargetVel == 1
    printf("goPathVel:%f\t pid.size:%f\t pid.vel:%f\t omega:%f\n", goPathVel.size, adjustVector.transVel.size,adjustVector.transVel.dir, omega);
#endif


}